/*
 * Copyright (c) 2011-2014, fortiss GmbH.
 * Licensed under the Apache License, Version 2.0.
 *
 * Use, modification and distribution are subject to the terms specified
 * in the accompanying license file LICENSE.txt located at the root directory
 * of this software distribution. A copy is available at
 * http://chromosome.fortiss.org/.
 *
 * This file is part of CHROMOSOME.
 *
 * $Id: askSumFunction.cpp 7805 2014-03-13 09:54:35Z geisinger $
 */

/**
 * \file
 *         Source file for function askSum in component proxy.
 *
 * \author
 *         This file has been generated by the CHROMOSOME Modeling Tool (XMT)
 *         (fortiss GmbH).
 */

/******************************************************************************/
/***   Includes                                                             ***/
/******************************************************************************/
#include "rOSGateway/adv/proxy/include/askSumFunction.h"

#include "rOSGateway/adv/proxy/include/askSumFunctionWrapper.h"
#include "rOSGateway/adv/proxy/include/proxyComponent.h"
#include "rOSGateway/adv/proxy/include/proxyComponentWrapper.h"
#include "rOSGateway/adv/proxy/include/proxyManifest.h"

#include "xme/core/logUtils.h"

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_ASKSUMFUNCTION_C_INCLUDES) ENABLED START
#pragma GCC diagnostic ignored "-Wshadow" //TODO find better solution
#include "ros/ros.h"
#include "ros/callback_queue.h"

#include "xme/defines.h"
#include "xme/hal/include/mem.h" //TODO
#include "xme/hal/include/ros.h" //TODO
#include "xme/hal/include/sched.h" //TODO
#include "xme/hal/include/sleep.h" //TODO
#include "xme/hal/include/time.h" //TODO

#include "ros_system/AddTwoInts.h"
// PROTECTED REGION END

/******************************************************************************/
/***   Definitions                                                          ***/
/******************************************************************************/

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_ASKSUMFUNCTION_C_DEFINITIONS) ENABLED START
// PROTECTED REGION END

/******************************************************************************/
/***   Variables                                                            ***/
/******************************************************************************/
/**
 * \brief  Variable holding the value of the required output port 'getSum'.
 *
 * \details If necessary initialize this in the init function.
 *          The value of this variable will be written to the port at the end of
 *          the step function.
 */
static ROSGateway_topic_sumRequest_t
portGetSumData;

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_ASKSUMFUNCTION_C_VARIABLES) ENABLED START
static rOSGateway_adv_proxy_proxyComponent_config_t* proxyComponentConfig = NULL;

static xme_hal_sched_taskHandle_t rosTaskHandle = XME_HAL_SCHED_INVALID_TASK_HANDLE;

ROSGateway_topic_sumRequest_t* request = NULL;

static ros::NodeHandle* nodeHandle;
static ros::CallbackQueue callbackQueue;

static ros::ServiceServer add_srv;
// PROTECTED REGION END

/******************************************************************************/
/***   Prototypes                                                           ***/
/******************************************************************************/

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_ASKSUMFUNCTION_C_PROTOTYPES) ENABLED START
static void runROSTaskCallback(void* userData);
static bool addRequest(ros_system::AddTwoInts::Request &req, ros_system::AddTwoInts::Response &res);
// PROTECTED REGION END

/******************************************************************************/
/***   Implementation                                                       ***/
/******************************************************************************/
xme_status_t
rOSGateway_adv_proxy_askSumFunction_init
(
    rOSGateway_adv_proxy_proxyComponent_config_t* const componentConfig
)
{
    // PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_ASKSUMFUNCTION_INITIALIZE_C) ENABLED START
    proxyComponentConfig = componentConfig;

    nodeHandle = new ros::NodeHandle();

    ros::AdvertiseServiceOptions add_srv_options = ros::AdvertiseServiceOptions::create<ros_system::AddTwoInts>("add_two_ints", addRequest, ros::VoidPtr(), &callbackQueue);
    add_srv = nodeHandle->advertiseService(add_srv_options);

	rosTaskHandle = xme_hal_sched_addTask(xme_hal_time_timeIntervalFromMilliseconds(0), xme_hal_time_timeIntervalFromMilliseconds(50), 0, runROSTaskCallback, NULL);
	if (XME_HAL_SCHED_INVALID_TASK_HANDLE == rosTaskHandle)
	{
		XME_EXIT(EXIT_FAILURE, "Error creating runROSThread\n");
	}

    return XME_STATUS_SUCCESS;
    // PROTECTED REGION END
}

void
rOSGateway_adv_proxy_askSumFunction_step
(
    rOSGateway_adv_proxy_proxyComponent_config_t* const componentConfig
)
{
    xme_status_t status[1];
    
    ROSGateway_topic_sumRequest_t* portGetSumDataPtr = &portGetSumData;
    
    {
        // PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_ASKSUMFUNCTION_STEP_C) ENABLED START
        XME_UNUSED_PARAMETER(componentConfig);

        if (NULL != request)
        {
            xme_fallback_printf("Proxy: askSum\n");

            //Forward request
            portGetSumData.a = request->a;
            portGetSumData.b = request->b;

        	xme_hal_mem_free(request);
        	request = NULL;
        }
        else
        {
        	//Stop XME from sending request
        	portGetSumDataPtr = NULL;
        }
        // PROTECTED REGION END
    }
    
    status[0] = rOSGateway_adv_proxy_proxyComponentWrapper_writePortGetSum(portGetSumDataPtr);
    
    {
        // PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_ASKSUMFUNCTION_STEP_2_C) ENABLED START
    	if (XME_STATUS_SUCCESS != status[0])
		{
			XME_LOG(XME_LOG_ERROR, "askSumFunction: Error sending request!\n");
		}
        // PROTECTED REGION END
    }
}

void
rOSGateway_adv_proxy_askSumFunction_fini
(
    rOSGateway_adv_proxy_proxyComponent_config_t* const componentConfig
)
{
    // PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_ASKSUMFUNCTION_TERMINATE_C) ENABLED START
    XME_UNUSED_PARAMETER(componentConfig);
    
	delete nodeHandle;
	nodeHandle = NULL;
    // PROTECTED REGION END
}

// PROTECTED REGION ID(ROSGATEWAY_ADV_PROXY_ASKSUMFUNCTION_IMPLEMENTATION_C) ENABLED START
static void runROSTaskCallback(void* userData)
{
	XME_UNUSED_PARAMETER(userData);

	if (xme_hal_ros_ok())
	{
		callbackQueue.callAvailable(ros::WallDuration(0.01));
	}
	else
	{
		XME_EXIT(EXIT_FAILURE, "ROS has been shut-down!\n");
	}
}

static
bool
addRequest(ros_system::AddTwoInts::Request &req, ros_system::AddTwoInts::Response &res)
{
	XME_UNUSED_PARAMETER(req);

	xme_fallback_printf("Proxy: Received add request\n");

	request = (ROSGateway_topic_sumRequest_t*)xme_hal_mem_alloc(sizeof(ROSGateway_topic_sumRequest_t));
	request->a = req.a;
	request->b = req.b;

	while (NULL == proxyComponentConfig->response)
	{
		xme_hal_sleep_sleep(xme_hal_time_timeIntervalFromMilliseconds(100));
	}

	xme_fallback_printf("Proxy: Send add response\n");
	res.sum = proxyComponentConfig->response->sum;
	xme_hal_mem_free((void*)proxyComponentConfig->response);
	proxyComponentConfig->response = NULL;

	xme_hal_sleep_sleep(xme_hal_time_timeIntervalFromMilliseconds(5000));

	return true;
}
// PROTECTED REGION END
